package com.google.android.apps.skipbot;

import android.os.AsyncTask;

/**
 * Class that implements a robot movement sequence for standing.
 */
public class SkipperStandSequence extends SkipperMovementSequence {

  /**
   * Creates a stand sequence for the Skipper robot.
   * 
   * @param skipperRobotConfig robot configuration
   * @param robotController robot controller that enables robot movements
   */
  public SkipperStandSequence(SkipperRobotConfigInterface skipperRobotConfig,
      SkipperRobotController robotController) {
    super(skipperRobotConfig, robotController);
  }

  @Override
  public void run(AsyncTask<RobotMovementSequence, Integer, Boolean> asyncTask) {
    try {
      final double scaleFactor = skipperRobotConfig.getScaleFactor();

      for (int i = 0; i < 5; ++i) {
        skipperRobotController.moveAllLegs(new double[][] { // Leg base position
            {3.5 * scaleFactor, 6.062 * scaleFactor, 1.0 * scaleFactor - i * scaleFactor}, // Leg
                                                                                           // 0
                {7 * scaleFactor, 0, 1.0 - i}, // Leg 1
                {3.5 * scaleFactor, -6.062 * scaleFactor, 1.0 * scaleFactor - i * scaleFactor}, // Leg
                                                                                                // 2
                {-3.5 * scaleFactor, -6.062 * scaleFactor, 1.0 * scaleFactor - i * scaleFactor}, // Leg
                                                                                                 // 3
                {-7 * scaleFactor, 0, 1.0 - i}, // Leg 4
                {-3.5 * scaleFactor, 6.062 * scaleFactor, 1.0 * scaleFactor - i * scaleFactor}, // Leg
                                                                                                // 5
            });
        Thread.sleep(100);
      }

      skipperRobotController.moveLeg(0, new double[] {3.5 * scaleFactor, 6.062 * scaleFactor,
          -1.5 * scaleFactor});
      skipperRobotController.moveLeg(2, new double[] {3.5 * scaleFactor, -6.062 * scaleFactor,
          -1.5 * scaleFactor});
      skipperRobotController.moveLeg(4, new double[] {-7 * scaleFactor, 0, -1.5 * scaleFactor});
      Thread.sleep(500);
      skipperRobotController.moveLeg(0, new double[] {3.5 * scaleFactor, 6.062 * scaleFactor,
          -3 * scaleFactor});
      skipperRobotController.moveLeg(2, new double[] {3.5 * scaleFactor, -6.062 * scaleFactor,
          -3 * scaleFactor});
      skipperRobotController.moveLeg(4, new double[] {-7 * scaleFactor, 0, -3 * scaleFactor});
      Thread.sleep(500);
      skipperRobotController.moveLeg(1, new double[] {7 * scaleFactor, 0, -1.5 * scaleFactor});
      skipperRobotController.moveLeg(3, new double[] {-3.5 * scaleFactor, -6.062 * scaleFactor,
          -1.5 * scaleFactor});
      skipperRobotController.moveLeg(5, new double[] {-3.5 * scaleFactor, 6.062 * scaleFactor,
          -1.5 * scaleFactor});
      Thread.sleep(500);
      skipperRobotController.moveLeg(1, new double[] {7 * scaleFactor, 0, -3 * scaleFactor});
      skipperRobotController.moveLeg(3, new double[] {-3.5 * scaleFactor, -6.062 * scaleFactor,
          -3 * scaleFactor});
      skipperRobotController.moveLeg(5, new double[] {-3.5 * scaleFactor, 6.062 * scaleFactor,
          -3 * scaleFactor});
      Thread.sleep(500);

    } catch (InterruptedException ignore) {
      return;
    } finally {
      // TODO(bbrown): Move into a known good stop position.
    }
  }

}
